
#include "bits/stdc++.h"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Cholesky"


class modelType
{
private:
    /* data */
public:
    modelType(){}
    ~modelType(){}

    virtual void solve(std::vector<std::vector<double>> inX, std::vector<std::vector<double>> inY) = 0; // 模型求解器
    virtual void evaluate(std::vector<std::vector<double>> & inX, std::vector<std::vector<double>> & inY, double th) = 0;// 模型评价
};


// 定义AX = B的这种模型

class lineType: public modelType
{
public:
    typedef Eigen::Matrix<double, 12, 12> M12_12;
    typedef Eigen::Matrix<double, 12, 1> M12_1;
    typedef Eigen::Matrix<double, 3, 1> M3_1;
    typedef Eigen::Matrix<double, 3, 3> M3_3;
    
    M3_3 mBestA;
    M3_1 mBestB;
    double mMaxPoint = 0;

    M12_1 res;
public:
    lineType(/* args */){}
    ~lineType(){}
    // 根据参数求解模型
    void solve(std::vector<std::vector<double>> inX, std::vector<std::vector<double>> inY) override{
        M12_12 A = M12_12::Zero();
        M12_1 B = M12_1::Zero();

        for(int i = 0; i < inX.size(); i++){
            A.block<1, 12>(i*3 + 0, 0) << inX[i][0], inX[i][1], inX[i][2], 0, 0, 0, 0, 0, 0, 1., 0., 0.;
            A.block<1, 12>(i*3 + 1, 0) << 0, 0, 0, inX[i][0], inX[i][1], inX[i][2], 0, 0, 0, 0., 1., 0.;
            A.block<1, 12>(i*3 + 2, 0) << 0, 0, 0, 0, 0, 0, inX[i][0], inX[i][1], inX[i][2], 0., 0., 1.;
            B(i*3+0, 0) = inY[i][0];
            B(i*3+1, 0) = inY[i][1];
            B(i*3+2, 0) = inY[i][2];
        }

        // std::cout << "A = " << std::endl << A << std::endl;
        // std::cout << "B = " << std::endl << B.transpose() << std::endl;
        // std::cout << "true B" << std::endl << (A * trueX).transpose() << std::endl;
        //  求解A X = B
        res = (A.transpose()*A).ldlt().solve(A.transpose() * B);
        // std::cout << "cal B" << std::endl << (A * res).transpose() << std::endl;
        // // debug
        // std::cout << res.transpose() << std::endl;
    }
    M3_1 coverge(std::vector<double> & in){
        return {in[0], in[1], in[2]};
    }
    // 对所有模型进行计算
    void evaluate(std::vector<std::vector<double>> & inX, std::vector<std::vector<double>> & inY, double th) override{
        M3_3 Anow;
        Anow << res(0,0), res(1,0), res(2,0), res(3,0), res(4,0), res(5,0), res(6,0), res(7,0), res(8,0);
        M3_1 Bnow;
        Bnow << res(9,0), res(10,0), res(11,0); 
        int nowSum = 0;
        double sumError = 0.;
        for(size_t i = 0; i < inX.size(); ++i){
            M3_1 expNow = coverge(inY[i]);
            M3_1 expX = coverge(inX[i]);
            M3_1 y = Anow * expX + Bnow;
            if((y-expNow).norm()< th){
                nowSum++;
                //sumError += (y-expNow).norm();
            }
        }
        if(nowSum <= 0) return;
        if(nowSum > mMaxPoint){
            mMaxPoint = nowSum;
            mBestA = Anow;
            mBestB = Bnow;
        }
    }
};
